#include "ms575_algfrontse3se4se5se6se7se8edgeouter.h"
using namespace MeasurE575;

AlgFrontSE3SE4SE5SE6SE7SE8EdgeOuter::AlgFrontSE3SE4SE5SE6SE7SE8EdgeOuter(int i):AlgBase(i)
{

}

void AlgFrontSE3SE4SE5SE6SE7SE8EdgeOuter::operator()(cv::Mat &srcImg,
                                                     int id,
                                                     std::map<string, float> &measRes,
                                                     int fixtureId)
{
//front SE3->13,SE4->14,SE5->15,SE6->16 ,SE7 ->17
  //front SE345678 -> 41,42,43,44,45,46
    std::vector<int> paras3,paras4,paras4t,paras5,paras6,paras6t,paras6tt,paras7,paras8;
    for(int j=0;j< m_dataAllVec[41].m_edgeParam.cols;j++)
    {
        paras3.push_back(m_dataAllVec[41].m_edgeParam.at<int>(1,j));
        paras4.push_back(m_dataAllVec[42].m_edgeParam.at<int>(4,j));
        paras4t.push_back(m_dataAllVec[42].m_edgeParam.at<int>(5,j));
        paras5.push_back(m_dataAllVec[43].m_edgeParam.at<int>(3,j));
        paras6.push_back(m_dataAllVec[44].m_edgeParam.at<int>(2,j));
        paras6t.push_back(m_dataAllVec[44].m_edgeParam.at<int>(7,j));
        paras6tt.push_back(m_dataAllVec[44].m_edgeParam.at<int>(8,j));
        paras7.push_back(m_dataAllVec[45].m_edgeParam.at<int>(0,j));
        paras8.push_back(m_dataAllVec[46].m_edgeParam.at<int>(6,j));
    }
    std::vector<cv::Point2f> resultSE3,resultSE4,resultSE4t,resultSE5,resultSE7,resultSE6,resultSE6t,resultSE6tt,resultSE8;
    std::vector<cv::Rect> rangSE8Vec,rangSE3Vec, rangSE4Vec, rangSE4tVec,rangSE5Vec, rangSE6Vec,rangSE6tVec,rangSE6ttVec, rangSE7Vec;




    {
        TemplateMatch(srcImg,m_dataAllVec[41],rangSE3Vec);
        edgedetect obj;
        obj.NewKirschEdgeOuter(srcImg(rangSE3Vec[0]),paras3,resultSE3);
        for(auto &pt1:resultSE3)
        {
            pt1.x+=rangSE3Vec[0].x;
            pt1.y+=rangSE3Vec[0].y;
        }
    }
    {
        TemplateMatch(srcImg,m_dataAllVec[42],rangSE4Vec);
        edgedetect obj;
        obj.NewKirschEdgeOuter(srcImg(rangSE4Vec[0]),paras4,resultSE4);
        for(auto &pt1:resultSE4)
        {
            pt1.x+=rangSE4Vec[0].x;
            pt1.y+=rangSE4Vec[0].y;
        }
    }

    {
        TemplateMatch(srcImg,m_dataAllVec[42],rangSE4tVec);
        edgedetect obj;
        obj.LDmeasure(srcImg(rangSE4tVec[0]),paras4t,resultSE4t);
        for(auto &pt1:resultSE4t)
        {
            pt1.x+=rangSE4tVec[0].x;
            pt1.y+=rangSE4tVec[0].y;
        }
    }

    {
        TemplateMatch(srcImg,m_dataAllVec[43],rangSE5Vec);
        edgedetect obj;
        obj.NewKirschEdgeOuter(srcImg(rangSE5Vec[0]),paras5,resultSE5);
        for(auto &pt1:resultSE5)
        {
            pt1.x+=rangSE5Vec[0].x;
            pt1.y+=rangSE5Vec[0].y;
        }
    }

    {
        TemplateMatch(srcImg,m_dataAllVec[44],rangSE6Vec);
        edgedetect obj;
        obj.NewKirschEdgeOuter(srcImg(rangSE6Vec[0]),paras6,resultSE6);
        for(auto &pt1:resultSE6)
        {
            pt1.x+=rangSE6Vec[0].x;
            pt1.y+=rangSE6Vec[0].y;
        }
    }

    {
        TemplateMatch(srcImg,m_dataAllVec[44],rangSE6tVec);
        edgedetect obj;
        obj.LDmeasure(srcImg(rangSE6tVec[0]),paras6t,resultSE6t);
        for(auto &pt1:resultSE6t)
        {
            pt1.x+=rangSE6tVec[0].x;
            pt1.y+=rangSE6tVec[0].y;
        }
    }

    {
        TemplateMatch(srcImg,m_dataAllVec[44],rangSE6ttVec);
        edgedetect obj;
        obj.LDmeasure(srcImg(rangSE6ttVec[0]),paras6tt,resultSE6tt);
        for(auto &pt1:resultSE6tt)
        {
            pt1.x+=rangSE6ttVec[0].x;
            pt1.y+=rangSE6ttVec[0].y;
        }
    }

    {
        TemplateMatch(srcImg,m_dataAllVec[45],rangSE7Vec);
        edgedetect obj;
        obj.NewKirschEdgeOuter(srcImg(rangSE7Vec[0]),paras7,resultSE7);
        for(auto &pt1:resultSE7)
        {
            pt1.x+=rangSE7Vec[0].x;
            pt1.y+=rangSE7Vec[0].y;
        }
    }


    {
        TemplateMatch(srcImg,m_dataAllVec[46],rangSE8Vec);
        edgedetect obj;
        obj.LDmeasure(srcImg(rangSE8Vec[0]),paras8,resultSE8);
        for(auto &pt1:resultSE8)
        {
            pt1.x+=rangSE8Vec[0].x;
            pt1.y+=rangSE8Vec[0].y;
        }
    }


    std::unique_lock<std::mutex> lock(*AlgBase::mtxptrVec[id]);
    while (!AlgBase::flagVec[id])
    {
        (*AlgBase::cvarptrVec[id]).wait(lock);
    }
    omp_set_num_threads(6);
#pragma omp parallel sections
    {
        #pragma omp section
          {
            for(auto &pt2:resultSE3)
            {
                cv::Point2f ptsOut;
                CoordinateTransformationRelateToDot(datumBackVec[id], pt2,ptsOut);
                pt2=ptsOut;
            }
          }
        #pragma omp section
        {
            for(auto &pt2:resultSE4)
            {
                cv::Point2f ptsOut;
                CoordinateTransformationRelateToDot(datumBackVec[id], pt2,ptsOut);
                pt2=ptsOut;
            }
         }
        #pragma omp section
        {
            for(auto &pt2:resultSE4t)
            {
                cv::Point2f ptsOut;
                CoordinateTransformationRelateToDot(datumBackVec[id], pt2,ptsOut);
                pt2=ptsOut;
            }
         }
        #pragma omp section
          {
            for(auto &pt2:resultSE5)
            {
                cv::Point2f ptsOut;
                CoordinateTransformationRelateToDot(datumBackVec[id], pt2,ptsOut);
                pt2=ptsOut;
            }
          }
        #pragma omp section
          {
            for(auto &pt2:resultSE6)
            {
                cv::Point2f ptsOut;
                CoordinateTransformationRelateToDot(datumBackVec[id], pt2,ptsOut);
                pt2=ptsOut;
            }
                  }
        #pragma omp section
          {
            for(auto &pt2:resultSE6t)
            {
                cv::Point2f ptsOut;
                CoordinateTransformationRelateToDot(datumBackVec[id], pt2,ptsOut);
                pt2=ptsOut;
            }
                  }
        #pragma omp section
        {
        for(auto &pt2:resultSE6tt)
        {
            cv::Point2f ptsOut;
            CoordinateTransformationRelateToDot(datumBackVec[id], pt2,ptsOut);
            pt2=ptsOut;
        }
              }
        #pragma omp section
        {
            for(auto &pt2:resultSE7)
            {
                cv::Point2f ptsOut;
                CoordinateTransformationRelateToDot(datumBackVec[id], pt2,ptsOut);
                pt2=ptsOut;
            }
         }
        #pragma omp section
        {
            for(auto &pt2:resultSE8)
            {
                cv::Point2f ptsOut;
                CoordinateTransformationRelateToDot(datumBackVec[id], pt2,ptsOut);
                pt2=ptsOut;
            }
         }
     }

    // go on coding resultSE3,resultSE4,resultSE4t,resultSE5,resultSE7,resultSE6,resultSE8;
   //std::cout<<"SE7SE9 "<<resultSE7[0].x<<endl;

    edgedetect obj;
    float fai2=0;
    for(int i=0;i<resultSE7.size();++i){
        fai2+=resultSE7[i].x/resultSE7.size();
    }

    std::string prefix = m_xmlFileNamePre + to_string(fixtureId) + "_";
    measRes.insert(std::make_pair(string("FAI2-SE7-X-distance_pixel"), fai2));
    float fai2se7 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI2-SE7-X-distance")], fai2);
    measRes.insert(std::make_pair(string("FAI2-SE7-X-distance_real"), fai2se7));



    float fai3 = 0;
    for(int i = 0; i < resultSE6.size(); ++i) {

        std::string fai3Se6Str = string("FAI3-SE6-Y" + std::to_string(i+1) + "-distance");
        measRes.insert(std::make_pair(string(fai3Se6Str + "_pixel"), resultSE6[i].y));
        float fai3Sey = CalcMeasureDistance(m_phypixpara[prefix + fai3Se6Str], resultSE6[i].y);
        measRes.insert(std::make_pair(string(fai3Se6Str + "_real"), fai3Sey));

        fai3 += resultSE6[i].y/resultSE6.size();
    }

    measRes.insert(std::make_pair(string("FAI3-SE6-Y-distance_pixel"), fai3));
    float fai3se6 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI3-SE6-Y-distance")], fai3);
    measRes.insert(std::make_pair(string("FAI3-SE6-Y-distance_real"), fai3se6));

    float fai4se3 = 0;
    for(int i = 0; i < resultSE3.size(); ++i){
        fai4se3 += resultSE3[i].y/resultSE3.size();
    }

    float fai4se6 = 0;
    for(int i = 0; i < resultSE6.size(); ++i){
        std::string fai4Se6Str = string("FAI4-SE3-SE6-Y" + std::to_string(i+1) + "-distance");
        measRes.insert(std::make_pair(string(fai4Se6Str + "_pixel"), fabs(fai4se6 - resultSE6t[i].y)));
        float fai4Se3Se6y = CalcMeasureDistance(m_phypixpara[prefix + fai4Se6Str], fabs(fai4se6 - resultSE6t[i].y));
        measRes.insert(std::make_pair(string(fai4Se6Str + "_real"), fai4Se3Se6y));

        fai4se6+=resultSE6t[i].y/resultSE6t.size();
    }

    measRes.insert(std::make_pair(string("FAI4-SE3-SE6-Y-distance_pixel"), fabs(fai4se3-fai4se6)));
    float fai4 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI4-SE3-SE6-Y-distance")], fabs(fai4se3-fai4se6));
    measRes.insert(std::make_pair(string("FAI4-SE3-SE6-Y-distance_real"), fai4));


    float fai5 = 0;
    for(int i = 0; i < resultSE5.size(); ++i) {
        fai5 += resultSE5[i].y/resultSE5.size();
    }

    measRes.insert(std::make_pair(string("FAI5-SE5-Y-distance_pixel"), fai5));
    float fai5se5 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI5-SE5-Y-distance")], fai5);
    measRes.insert(std::make_pair(string("FAI5-SE5-Y-distance_real"), fai5se5));

    float fai7 = 0;
    for(int i = 0; i < resultSE4.size(); ++i) {

        std::string fai7Se4Str = string("FAI7-SE4-X" + std::to_string(i+1) + "-distance");
        measRes.insert(std::make_pair(string(fai7Se4Str + "_pixel"), resultSE4[i].x));
        float fai7Se4x = CalcMeasureDistance(m_phypixpara[prefix + fai7Se4Str], resultSE4[i].x);
        measRes.insert(std::make_pair(string(fai7Se4Str + "_real"), fai7Se4x));

        fai7 += resultSE4[i].x/resultSE4.size();
    }

    measRes.insert(std::make_pair(string("FAI7-SE4-X-distance_pixel"), fai7));
    float fai7se4 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI7-SE4-X-distance")], fai7);
    measRes.insert(std::make_pair(string("FAI7-SE4-X-distance_real"), fai7se4));


    float linee=obj.GetParallelism(resultSE6tt, 2);
    measRes.insert(std::make_pair(string("Line-E-parallelism_pixel"), linee));
    float lineer = CalcMeasureDistance(m_phypixpara[prefix + std::string("Line-E-parallelism")], linee);
    measRes.insert(std::make_pair(string("Line-E-parallelism_real"), lineer));

    float linef=obj.GetParallelism(resultSE8, 1);
    measRes.insert(std::make_pair(string("Line-F-parallelism_pixel"), linef));
    float linefr = CalcMeasureDistance(m_phypixpara[prefix + std::string("Line-F-parallelism")], linef);
    measRes.insert(std::make_pair(string("Line-F-parallelism_real"), linefr));

    float lineg=obj.GetParallelism(resultSE4t,1);
    measRes.insert(std::make_pair(string("Line-G-parallelism_pixel"), lineg));
    float linegr = CalcMeasureDistance(m_phypixpara[prefix + std::string("Line-G-parallelism")], lineg);
    measRes.insert(std::make_pair(string("Line-G-parallelism_real"), linegr));
}
